﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace MotionPlanning.MotionPlanningComponents.MotionPlanningSystems
{
    ///
    /// System for Point2D
    ///
    public class System_Point2D : RoboticSystemComponent
    {
        public override bool LoadingSystemParameters(string fileName)
        {
            base.LoadingSystemParameters(fileName);
            //if (this.System_Type.Equals("Robotics_Point2D", StringComparison.OrdinalIgnoreCase) == false)
                //throw new Exception("input file does not contain Point2D system");
            return true;
        }
        public override bool Propagate(double[] q_start, double[] u, double duration, ref double[] q_end, ref double step_cost)
        {
            q_end[0] = q_start[0] + u[0] * duration;
            q_end[1] = q_start[1] + u[1] * duration;
            double length = Math.Sqrt(u[0] * u[0] + u[1] * u[1]);
            step_cost = duration * length;
            return true;
        }
        public override double Distance2_Topological(double[] x1, double[] x2)
        {
            double diff_q1 = (x1[0] - x2[0]) * q_scale[0];
            double diff_q2 = (x1[1] - x2[1]) * q_scale[1];
            return diff_q1 * diff_q1 + diff_q2 * diff_q2;
        }
        public override string ToString()
        {
            return "\n\t<System_Type>Robotics_Point2D</System_Type>" +
                    base.ToString();
        }

        override public bool Steering(double[] q_start, double[] q_end, ref double[] u, ref double duration, ref double edgeCost)
        {
            u[0] = q_end[0] - q_start[0];
            u[1] = q_end[1] - q_start[1];
            duration = 1;
            double length = Math.Sqrt(u[0] * u[0] + u[1] * u[1]);
            edgeCost = length;

            return true;
        }
    }
}
